/*
 * LocalGraph.cpp
 *
 *  Created on: 14/11/2011
 *      Author: will
 */

#include "LocalGraph.h"
#include "mex.h"

#include <queue>

using std::queue;

LocalGraph::LocalGraph(Particle* particle, double radius, unsigned int maxFrame) {

	// allocate queue and return value
	// queue for BFS, stack for DFS
	queue<pair<GraphNode*, Vector3d> > fringe;
	pair<map<GraphNode*, Vector3d>::iterator, bool> ret;
	numParticles = 0;
	//mexPrintf("Particle %X on edge %X\n",particle,particle->root);

	// enqueue root
	Vector3d origin(0, 0, pi * (double) particle->direction);
	origin = reverseModelFrac(origin, particle->root->odometry,
			particle->fraction);
	fringe.push(pair<GraphNode*, Vector3d> (particle->root->nodeFrom, origin));
	//mexPrintf("Added node %X to fringe\n",particle->root->nodeFrom);

	while (!fringe.empty()) {
		// pop element from fringe to map
		ret = nodes.insert(fringe.front());
		//mexPrintf("Attempted node %X result %d\n",(*(ret.first)).first,ret.second);
		fringe.pop();

		// if already in the map, skip
		if (!ret.second) {
			//mexPrintf("  Already in map, skipping...\n");
			continue;
		}

		GraphNode *node = (*(ret.first)).first;
		//mexPrintf("  Expanding nodes to %X\n",node);
		Vector3d position;

		// add nodes from
		GraphEdge *edge;
		for (vector<GraphEdge*>::iterator edgeIter = node->edgeFrom.begin(); edgeIter
				!= node->edgeFrom.end(); edgeIter++) {
			edge = *edgeIter;
			//mexPrintf("    Testing edge %X\n",edge);
			position = reverseModel((*(ret.first)).second, edge->odometry);
			if (edge->frameNum < maxFrame) {

				edges.insert(edge);
				numParticles += edge->particles.size();

				if (getRadius(position) < radius) {
					fringe.push(pair<GraphNode*, Vector3d> (edge->nodeFrom,
							position));
				} else {
					nodes.insert(pair<GraphNode*, Vector3d> (edge->nodeFrom,
							position));
				}
			}
		}

		//mexPrintf("  Expanding nodes from %X\n",node);
		// add nodes to
		for (vector<GraphEdge*>::iterator edgeIter = node->edgeTo.begin(); edgeIter
				!= node->edgeTo.end(); edgeIter++) {
			edge = *edgeIter;
			//mexPrintf("    Testing edge %X\n",edge);
			position = forwardModel((*(ret.first)).second, edge->odometry);
			if (edge->frameNum < maxFrame) {

				edges.insert(edge);
				numParticles += edge->particles.size();

				if (getRadius(position) < radius) {
					fringe.push(pair<GraphNode*, Vector3d> (edge->nodeTo,
							position));
				} else {
					nodes.insert(pair<GraphNode*, Vector3d> (edge->nodeTo,
							position));
				}
			}
		}
	}
	//mexPrintf("Local graph with %d nodes, %d edges, %d particles\n",nodes.size(),edges.size(),numParticles);
}

LocalGraph::LocalGraph(GraphNode* root, double radius) {

	// allocate queue and return value
	// queue for BFS, stack for DFS
	queue<pair<GraphNode*,Vector3d> > fringe;
	pair<map<GraphNode*,Vector3d>::iterator,bool> ret;

	// enqueue root
	Vector3d origin(0,0,0);
	fringe.push(pair<GraphNode*,Vector3d>(root,origin));

	while(!fringe.empty()) {
		// pop element from fringe to map
		ret = nodes.insert(fringe.front());
		fringe.pop();

		// if already in the map, skip
		if (!ret.second) {
			continue;
		}

		GraphNode* node = (*(ret.first)).first;
		Vector3d position;

		// add nodes from
		for (vector<GraphEdge*>::iterator edge = node->edgeFrom.begin(); edge
				!= node->edgeFrom.end(); edge++) {
			position = reverseModel((*(ret.first)).second, (*edge)->odometry);
			if (getRadius(position) < radius) {
				edges.insert(*edge);
				fringe.push(pair<GraphNode*, Vector3d> ((*edge)->nodeFrom,
						position));
			}
		}

		// add nodes to
		for (vector<GraphEdge*>::iterator edge = node->edgeTo.begin(); edge
				!= node->edgeTo.end(); edge++) {
			position = forwardModel((*(ret.first)).second, (*edge)->odometry);
			if (getRadius(position) < radius) {
				edges.insert(*edge);
				fringe.push(pair<GraphNode*, Vector3d> ((*edge)->nodeTo,
						position));
				mexPrintf("Added edge %X to node %X");
			}
		}
	}
}

LocalGraph::LocalGraph(GraphNode* root, unsigned int depth) {
	pair<map<GraphNode*,Vector3d>::iterator,bool> ret;

	// push root to map
	Vector3d origin(0,0,0);
	ret = nodes.insert(pair<GraphNode*,Vector3d>(root,origin));

	// add nodes from
	for (vector<GraphEdge*>::iterator edge = root->edgeFrom.begin(); edge
			!= root->edgeFrom.end(); edge++) {
		edges.insert(*edge);
		ret = nodes.insert(pair<GraphNode*, Vector3d> ((*edge)->nodeFrom,
				reverseModel(origin, (*edge)->odometry)));
	}

	// add nodes to
	for (vector<GraphEdge*>::iterator edge = root->edgeTo.begin(); edge
			!= root->edgeTo.end(); edge++) {
		edges.insert(*edge);
		ret = nodes.insert(pair<GraphNode*, Vector3d> ((*edge)->nodeTo, forwardModel(
				origin, (*edge)->odometry)));
	}

}

LocalGraph::LocalGraph() {
}

LocalGraph::~LocalGraph() {

}

LocalGraph::LocalGraph(const LocalGraph & other) {
	nodes = other.nodes;
	edges = other.edges;
	numParticles = other.numParticles;
}

LocalGraph & LocalGraph::operator=(const LocalGraph & other) {
	nodes = other.nodes;
	edges = other.edges;
	numParticles = other.numParticles;
	return *this;
}
